#include "MCParallelConstraint.h"
#include <MCParallelConstraintVisual.h>

using namespace cg;

MCParallelConstraint::MCParallelConstraint(MCSegmentVisual *s1, MCSegmentVisual *s2) :
    _segment1(s1),
    _segment2(s2),
    _rootDir1(s1->lineSegment().direction()),
    _rootDir2(s2->lineSegment().direction()),
    _visual(new MCParallelConstraintVisual(_segment1->canvas(), this))
{
    _segment1->canvas()->addVisualItem(_visual);
    _solver = CreateSolver();
    resolve();
}

MCParallelConstraint::~MCParallelConstraint()
{
    RemoveSolver(_solver);
}

void MCParallelConstraint::resolve()
{
    if (_segment2->lineSegment().direction() != _rootDir2)
    {   
        MCLineSegment2D mcLineSegment1 = _segment1->lineSegment();
        MCVector2 vec1 = _segment2->lineSegment().direction();
        MCVector2 vec2 = _segment1->lineSegment().direction();
        makeCollinier(vec1, vec2);

        _segment1->update(mcLineSegment1.startPoint(), vec2, mcLineSegment1.length());
        _rootDir1 = _segment1->lineSegment().direction();
        _rootDir2 = _segment2->lineSegment().direction();
    }
    else if (_segment1->lineSegment().direction() != _rootDir1)
    {
        MCLineSegment2D mcLineSegment2 = _segment2->lineSegment();
        MCVector2 vec1 = _segment1->lineSegment().direction();
        MCVector2 vec2 = _segment2->lineSegment().direction();
        makeCollinier(vec1, vec2);

        _segment2->update(mcLineSegment2.startPoint(), vec2, mcLineSegment2.length());
        _rootDir1 = _segment1->lineSegment().direction();
        _rootDir2 = _segment2->lineSegment().direction();
    }
}


void MCParallelConstraint::makeCollinier(MCVector2 &dir1, MCVector2 &dir2)
{
    Var x1 = CreateVariable(_solver, dir1.x); Var y1 = CreateVariable(_solver, dir1.y);
    Var x2 = CreateVariable(_solver, dir2.x); Var y2 = CreateVariable(_solver, dir2.y);

    Expr expression = x1 * y2 - y1 * x2;

    Eq equation = AddEquation(_solver, expression);

    if (Solve(_solver))
    {
        dir1.x = VariableValue(_solver, x1); dir1.y = VariableValue(_solver, y1);
        dir2.x = VariableValue(_solver, x2); dir2.y = VariableValue(_solver, y2);
    }

    dir1 = dir1.normalize();
    dir2 = dir2.normalize();

    RemoveEquation(_solver, equation);
    RemoveVariable(_solver, x1); RemoveVariable(_solver, x2); RemoveVariable(_solver, y1); RemoveVariable(_solver, y2);
}